/**
* @file ata.c
* ATA (AT Attachment) driver implementation.
*
* To make first version of this driver I have gotten some inspiration
* from Minirighi OS, Minix, Helen OS and Hale Landis (www.ata-atapi.com) drivers.
* Thanks!
*
* Current status of implementation:
* Detection of ATA controllers based on PCI emuration and device class/subclass.
* Detection of ATA devices based on software reset and test write/read to ATA channels.
* Commands INDETIFY DEVICE, IDENTIFY PACKET DEVICE according to ATA-6 standart.
* ATA-6 PIO mode: READ MULTIPLE EXT, WRITE MULTIPLE EXT (LBA48 support).
*
* Copyrights 2011 Michal Saman, m.saman@designplus.cz.
* This source code is release under the Apache License 2.0.m
* http://www.apache.org/licenses/LICENSE-2.0
*/

#include <env.h>
#include <errno.h>
#include <drivers/pci/pci.h>
#include <drivers/ata/ata.h>
#include <hal/hal.h>
#include <mem/kheap.h>
#include <main/kprintf.h>

/* Global handler of ATA controllers/channels/devices */
PUBLIC struct ata	_ata;

/* Holds base addresses for legacy ATA controllers in system. */
PRIVATE ata_base_t	legacy_base[ATA_MAX_LEGACY_CTRL][ATA_CHANNELS_PER_LEGACY_CTRL] = {
		{
				{ATA_CMD_BASE_CHANNEL_1, ATA_CTRL_BASE_CHANNEL_1},
				{ATA_CMD_BASE_CHANNEL_2, ATA_CTRL_BASE_CHANNEL_2}
		}, {
				{ATA_CMD_BASE_CHANNEL_3, ATA_CTRL_BASE_CHANNEL_3},
				{ATA_CMD_BASE_CHANNEL_4, ATA_CTRL_BASE_CHANNEL_4}
		}
};

/* Hold IRQ numbers for legacy ATA controllers ins system */
PRIVATE uint16_t	legacy_irqs[ATA_MAX_LEGACY_CTRL][ATA_CHANNELS_PER_LEGACY_CTRL] = {
		{
				ATA_CHANNEL1_IRQ,
				ATA_CHANNEL2_IRQ
		},{
				ATA_CHANNEL3_IRQ,
				ATA_CHANNEL4_IRQ
		}
};

/* Array of cmd I/O ports offsets. */
PRIVATE uint16_t	io_ports_offset[ATA_CHANNEL_TOTAL_PORTS] = {
		ATA_REG_DATA_OFFSET,
		ATA_REG_ERR_OFFSET,
		ATA_REG_SC_OFFSET,
		ATA_REG_SN_OFFSET,
		ATA_REG_CL_OFFSET,
		ATA_REG_CH_OFFSET,
		ATA_REG_DH_OFFSET,
		ATA_REG_SR_CMD_OFFSET,
		ATA_REG_ASR_DEVC_OFFSET,
		ATA_REG_ADDR_OFFSET
};

PUBLIC void ata_init() {
	_ata.count = 0;
	ata_controllers_detect();
	ata_device_detect(true);
}

/**
 * This function is called directly by INT instruction (over IDT vector).
 * It's workaround for GCC compiler, because automatic stack framing is difficult to manage
 * and to return stack to original state. This empty function do only saves of all registers, stops interrupts and
 * call to real handler (whitch can use stack framing). Than restore registers and start intterupts again.
 */
void ata_irq_handler_c1_ch1() {
	__asm__ volatile ("cli");
	__asm__ volatile ("pushal");
	ata_irq_c1_ch1();
	__asm__ volatile ("popal");
	//__asm__ volatile ("movl %ebp, %esp");
	//__asm__ volatile ("popl %ebp");
	__asm__ volatile ("sti");
	__asm__ volatile ("iretl");
}

void ata_irq_handler_c1_ch2() {
	__asm__ volatile ("cli");
	__asm__ volatile ("pushal");
	ata_irq_c1_ch2();
	__asm__ volatile ("popal");
	//__asm__ volatile ("movl %ebp, %esp");
	//__asm__ volatile ("popl %ebp");
	__asm__ volatile ("sti");
	__asm__ volatile ("iretl");
}

void ata_irq_handler_c2_ch1() {
	__asm__ volatile ("cli");
	__asm__ volatile ("pushal");
	ata_irq_c2_ch1();
	__asm__ volatile ("popal");
	//__asm__ volatile ("movl %ebp, %esp");
	//__asm__ volatile ("popl %ebp");
	__asm__ volatile ("sti");
	__asm__ volatile ("iretl");
}

void ata_irq_handler_c2_ch2() {
	__asm__ volatile ("cli");
	__asm__ volatile ("pushal");
	ata_irq_c2_ch2();
	__asm__ volatile ("popal");
	//__asm__ volatile ("movl %ebp, %esp");
	//__asm__ volatile ("popl %ebp");
	__asm__ volatile ("sti");
	__asm__ volatile ("iretl");
}

void __attribute__ ((cdecl)) ata_irq_c1_ch1() {
	//kprintf("   (*** ATA C1,CH1 - IRQ CALL ***)\n");
	end_hw_interrupt(ATA_CHANNEL1_IRQ);
}
void __attribute__ ((cdecl)) ata_irq_c1_ch2() {
	//kprintf("   (*** ATA C1,CH2 - IRQ CALL ***)\n");
	end_hw_interrupt(ATA_CHANNEL2_IRQ);
}
void __attribute__ ((cdecl)) ata_irq_c2_ch1() {
	//kprintf("   (*** ATA C2,CH1 - IRQ CALL ***)\n");
	end_hw_interrupt(ATA_CHANNEL3_IRQ);
}
void __attribute__ ((cdecl)) ata_irq_c2_ch2() {
	//kprintf("   (*** ATA C2,CH2 - IRQ CALL ***)\n");
	end_hw_interrupt(ATA_CHANNEL4_IRQ);
}

void ata_driver_install(int irq) {
	switch (irq) {
		case ATA_CHANNEL1_IRQ :
			set_handler_interrupt(hal_pic_base0+irq, ata_irq_handler_c1_ch1);
			break;
		case ATA_CHANNEL2_IRQ :
			set_handler_interrupt(hal_pic_base0+irq, ata_irq_handler_c1_ch2);
			break;
		case ATA_CHANNEL3_IRQ :
			set_handler_interrupt(hal_pic_base0+irq, ata_irq_handler_c2_ch1);
			break;
		case ATA_CHANNEL4_IRQ :
			set_handler_interrupt(hal_pic_base0+irq, ata_irq_handler_c2_ch2);
	}
}

/**
 * Tries to detect all ATA controllers based on pci class and subclass.
 * Detected controllers are stored in global _ata structure.
 */
PUBLIC	void	ata_controllers_detect() {
	unsigned y, i, ata_ch, ata_c = 0;

	kprintf(">>> Searching... ", _ata.count);

	// list all devices on pci and tries to find ata controllers
	for (i=0; i < _pci.count; i++) {
		// try to identify by class and subclass
		if (_pci.pci_confs[i].base_class == PCI_MASS_STORAGE &&
				(_pci.pci_confs[i].sub_class == 0x01 || _pci.pci_confs[i].sub_class == 0x05))
			/* we found ATA controller */
			;
		else
			continue; /* this device is not ATA controller so continue to next... */

		ata_controller_t* ata_ctrls = (ata_controller_t *) kmalloc((i+1)*sizeof(ata_controller_t));			// temp array
		if (i>0) {
			memcpy(ata_ctrls, _ata.ata_ctrls, i*sizeof(ata_controller_t));			// copy old array to new larger array
			kfree(_ata.ata_ctrls);													// free old array !! TODO KHEAP free is unstable and buggy, if enable some data leaks...
		}
		_ata.ata_ctrls = ata_ctrls;												// change pointer to valid array
		ata_ctrls = NULL;														// discart temporary variable

		ata_c = _ata.count++;	// increase count of ATA controllers

		_ata.ata_ctrls[ata_c].device_id = _pci.pci_confs[i].device_id;
		_ata.ata_ctrls[ata_c].vendor_id = _pci.pci_confs[i].vendor_id;
		_ata.ata_ctrls[ata_c].pci_conf_space = &_pci.pci_confs[i];

		// try to detect controller's channels mode
		// CHANNEL 1-2
		for (ata_ch = 0; ata_ch < ATA_CHANNELS_PER_LEGACY_CTRL; ata_ch++)
			if (_pci.pci_confs[i].interface_code & ATA_CHANNEL1_COMP_MASK) {
				// NATIVE MODE ATA Controller (PCI)
				// TODO :)
				kprintf("Native mode controller, if: %x.\n", _pci.pci_confs[i].interface_code);
				for (y=0; y < ATA_DEVICES_PER_CHANNEL; y++)
					_ata.ata_ctrls[ata_c].channels[ata_ch].devices[y].device_type = ATA_DEV_NONE;
			} else {
				// COMPATABILITY MODE ATA Controller
				_ata.ata_ctrls[ata_c].channels[ata_ch].cmd_base = legacy_base[ata_c][ata_ch].cmd_base;
				_ata.ata_ctrls[ata_c].channels[ata_ch].ctrl_base = legacy_base[ata_c][ata_ch].ctrl_base;

				for (y=0; y < ATA_CHANNEL_MAX_CMD_PORTS; y++)
					_ata.ata_ctrls[ata_c].channels[ata_ch].io_ports[y] = legacy_base[ata_c][ata_ch].cmd_base + io_ports_offset[y];
				for (; y < ATA_CHANNEL_MAX_CMD_PORTS + ATA_CHANNEL_MAX_CTRL_PORTS; y++) {
					_ata.ata_ctrls[ata_c].channels[ata_ch].io_ports[y] = legacy_base[ata_c][ata_ch].ctrl_base + io_ports_offset[y];
					//kprintf("Port: %i, %x, %x\n", io_ports_offset[y], legacy_base[ata_c][ata_ch].ctrl_base, legacy_base[ata_c][ata_ch].ctrl_base + io_ports_offset[y]);
				}

				_ata.ata_ctrls[ata_c].channels[ata_ch].irq = legacy_irqs[ata_c][ata_ch];

				for (y=0; y < ATA_DEVICES_PER_CHANNEL; y++) {
					_ata.ata_ctrls[ata_c].channels[ata_ch].devices[y].device_type = ATA_DEV_NONE;
					_ata.ata_ctrls[ata_c].channels[ata_ch].devices[y].dh_reg = ATA_REG_DH_OBSOLETE | (y << 4);  // y move to 4 bit, it representing device priority. 0 - master, 1 - slave
					_ata.ata_ctrls[ata_c].channels[ata_ch].devices[y].channel_position = y;
					_ata.ata_ctrls[ata_c].channels[ata_ch].devices[y].device_channel = &(_ata.ata_ctrls[ata_c].channels[ata_ch]);
					//kprintf("Device: %i, dh_reg: %x\n", y, _ata.ata_ctrls[ata_c].channels[ata_ch].devices[y].dh_reg);
				}
			}
	}

	kprintf("DONE. Found %i controllers.\n", _ata.count);
}

/**
 * Write/read ATA controllers channels to check if
 * channel is working. Then tries to calls INDETIFY DEVICE to
 * check if device is connected and working and to obtain detail info about device.
 * Finally, software reset is called to ensure that the devices communicate.
 *
 * @param	use_interrupt		Assert interrupts upon comunnication with devices.
 * @return 	standart error codes (EOK, EBUSY)
 */
PUBLIC int ata_device_detect(bool use_interrupt) {
	unsigned device, ata_c, ata_ch;
	uint8_t	cl, ch, sr, sc, sn, dev_ctrl = 0;

	//dev_ctrl = ATA_REG_DEVC_HD15 | (use_interrupt ? 0 : ATA_REG_DEVC_NIEN);
	dev_ctrl = use_interrupt ? 0 : ATA_REG_DEVC_NIEN;

	for (ata_c=0; ata_c < _ata.count; ata_c++)
		for (ata_ch = 0; ata_ch < ATA_CHANNELS_PER_LEGACY_CTRL; ata_ch++) {
			// install irq handler for channel
			ata_driver_install(_ata.ata_ctrls[ata_c].channels[ata_ch].irq);
			for (device = 0; device < ATA_DEVICES_PER_CHANNEL; device++) {
				// select device
				if (select_device(device, &_ata.ata_ctrls[ata_c].channels[ata_ch], 0) == EBUSY)
					return EBUSY;

				// send controll register value
				outportb(_ata.ata_ctrls[ata_c].channels[ata_ch].io_ports[ATA_REG_DEVC], dev_ctrl);
				ATA_DELAY_400NS;

				// try to write some writable register, if some device exists on channel
				outportb(_ata.ata_ctrls[ata_c].channels[ata_ch].io_ports[ATA_REG_SC], 0x44);
				outportb(_ata.ata_ctrls[ata_c].channels[ata_ch].io_ports[ATA_REG_SN], 0xbb);
				outportb(_ata.ata_ctrls[ata_c].channels[ata_ch].io_ports[ATA_REG_SC], 0xbb);
				outportb(_ata.ata_ctrls[ata_c].channels[ata_ch].io_ports[ATA_REG_SN], 0x44);
				outportb(_ata.ata_ctrls[ata_c].channels[ata_ch].io_ports[ATA_REG_SC], 0x44);
				outportb(_ata.ata_ctrls[ata_c].channels[ata_ch].io_ports[ATA_REG_SN], 0xbb);

				sc = inportb(_ata.ata_ctrls[ata_c].channels[ata_ch].io_ports[ATA_REG_SC]);
				sn = inportb(_ata.ata_ctrls[ata_c].channels[ata_ch].io_ports[ATA_REG_SN]);

				if ( (sc == 0x44) && (sn == 0xbb) ) {
					// 01 - maybe there is some device, but we don't know what type
					_ata.ata_ctrls[ata_c].channels[ata_ch].devices[device].device_type = ATA_DEV_UNKNOWN;
					// 02 - try to send IDENTIFY command to device
					if (ata_indetify_device(device, &_ata.ata_ctrls[ata_c].channels[ata_ch]) != EOK)				// try normal device
						if (ata_indetify_packet_device(device, &_ata.ata_ctrls[ata_c].channels[ata_ch]) != EOK)		// try ATAPI device
							_ata.ata_ctrls[ata_c].channels[ata_ch].devices[device].device_type = ATA_DEV_NONE;
						else
							_ata.ata_ctrls[ata_c].channels[ata_ch].devices[device].device_type = ATA_DEV_PATAPI;
					else
						_ata.ata_ctrls[ata_c].channels[ata_ch].devices[device].device_type = ATA_DEV_PATA;
					// 03 - try to software reset device (only if previous identify command works)
					if (_ata.ata_ctrls[ata_c].channels[ata_ch].devices[device].device_type != ATA_DEV_NONE)
						if (ata_soft_reset(&_ata.ata_ctrls[ata_c].channels[ata_ch], use_interrupt) == EOK) {
							if (select_device(device, &_ata.ata_ctrls[ata_c].channels[ata_ch], 0) == EBUSY)
								return EBUSY;
							sc = inportb(_ata.ata_ctrls[ata_c].channels[ata_ch].io_ports[ATA_REG_SC]);
							sn = inportb(_ata.ata_ctrls[ata_c].channels[ata_ch].io_ports[ATA_REG_SN]);
							if ( (sc == 0x01) && (sn == 0x01) ) {
								cl = inportb(_ata.ata_ctrls[ata_c].channels[ata_ch].io_ports[ATA_REG_CL]);
								ch = inportb(_ata.ata_ctrls[ata_c].channels[ata_ch].io_ports[ATA_REG_CH]);
								sr = inportb(_ata.ata_ctrls[ata_c].channels[ata_ch].io_ports[ATA_REG_SR]);
								if ( (cl == 0x14) && (ch == 0xEB) )
									if (_ata.ata_ctrls[ata_c].channels[ata_ch].devices[device].device_type != ATA_DEV_PATAPI)
										kprintf("WARNING: soft reset determined dev PATAPI, it's different from IDENTIFY cmd.");
								else if ( (cl == 0x0) && (ch == 0x0) && (sr != 0x00) )
									if (_ata.ata_ctrls[ata_c].channels[ata_ch].devices[device].device_type != ATA_DEV_PATA)
										kprintf("WARNING: soft reset determined dev PATA, it's different from IDENTIFY cmd.");
								else if ( (cl == 0x3C) && (ch == 0xC3) )
									_ata.ata_ctrls[ata_c].channels[ata_ch].devices[device].device_type = ATA_DEV_SATA;			// we don't support yet
								else if ( (cl == 0x69) && (ch == 0x96) )
									_ata.ata_ctrls[ata_c].channels[ata_ch].devices[device].device_type = ATA_DEV_SATAPI;		// we don't support yet
								else
									_ata.ata_ctrls[ata_c].channels[ata_ch].devices[device].device_type = ATA_DEV_UNKNOWN; // there is something, but god knows what it is :)
							}
						}
				} else {
					// channel write register, don't works, so we don't found any device :(
					_ata.ata_ctrls[ata_c].channels[ata_ch].devices[device].device_type = ATA_DEV_NONE;
				}

			}

		}

	return EOK;
}

/**
 * Repeatebly read status register and
 * waint util some bits are set and some are reset.
 *
 * @param sr_set_mask		Bits that must be set (=1)
 * @param sr_reset_mas		Bist that must be reset (=0)
 * @param channel			ATA controller channel from which reads status register
 * @param *sr_reg			Pointer to save last status register, or NULL
 *
 * @return	EOK if success, ETIMEOUT if bit not equals in specified timeout.
 */
PRIVATE int ata_wait_for_status(uint8_t sr_set_mask, uint8_t sr_reset_mask, ata_channel_t * channel, uint8_t * sr_reg) {
	uint32_t i = 0;
	uint8_t sr;
	//kprintf("DRQSET: %x, BSYSET: %x\n", sr_set_mask, sr_reset_mask );
	do {
		if (i >= ATA_MAX_STATUS_WAIT)
			break;
		sr = inportb((*channel).io_ports[ATA_REG_SR]);		// read status register
		i++;
		//kprintf("I: %i - %x. ", i, sr & sr_set_mask);
	} while ( (sr & sr_set_mask) != sr_set_mask || (sr & sr_reset_mask) != 0 );

	if (sr_reg)
		*sr_reg = sr;

	if (i >= ATA_MAX_STATUS_WAIT)
		return ETIMEOUT;
	else
		return EOK;
}

/**
 * Select device at channel.
 *
 * @param	device_index	0 - master, 1 - slave
 * @param	channel			Link to channel with devices
 * @param	dh_modify		Modify DH register with this value
 *
 * @return	EOK, EBUSY
 */
PRIVATE int select_device(int device_index, ata_channel_t * channel, uint8_t dh_modify) {
	uint8_t sr_reg;
	if (ata_wait_for_status(0, ATA_REG_SR_BSY, channel, &sr_reg) != EOK)
		return EBUSY;
	outportb((*channel).io_ports[ATA_REG_DH], (*channel).devices[device_index].dh_reg | dh_modify);
	ATA_DELAY_400NS;
	if (ata_wait_for_status(0, ATA_REG_SR_BSY, channel, &sr_reg) != EOK)
		return EBUSY;
	return EOK;
}

/**
 * Identify device with CMD INDETIFY.
 * Raw data saves to channel->device->raw_ident.
 * Processed data saves to channel->device->device_info.
 *
 * @return standart errors codes (errno.h).
 */
int ata_indetify_device(int device_index, ata_channel_t * channel) {
	uint8_t sr_reg;
	unsigned i;

	//kprintf("DC port: %x\n", (*channel).io_ports[ATA_REG_DEVC]);
	outportb((*channel).io_ports[ATA_REG_DEVC], 0);

	if (ata_wait_for_status(0, ATA_REG_SR_BSY, channel, &sr_reg) != EOK)					// wait for bsy=0
		return ETIMEOUT;

	outportb((*channel).io_ports[ATA_REG_CMD], ATA_CMD_IDENTIFY_DEVICE);					// send command
	ATA_DELAY_400NS;

	if (ata_wait_for_status(0, ATA_REG_SR_BSY, channel, &sr_reg) != EOK)					// wait for bsy=0
		return ETIMEOUT;

	if ((sr_reg & ATA_REG_SR_ERR) != 0)
		return EIO;			// some error occurs - exit

	//kprintf("DRQ: %x\n", ATA_REG_SR_DRQ);
	if (ata_wait_for_status(ATA_REG_SR_DRQ, ATA_REG_SR_BSY, channel, &sr_reg) != EOK)		// wait for bsy=0, drq=1
		return ETIMEOUT;

	//kprintf("SR: %x, %x\n", sr_reg, sr_reg & ATA_REG_SR_DRQ);

	// read data from data register
	for (i = 0; i < ATA_INDETIFY_SIZE; i++) {
		((uint16_t *) &(*channel).devices[device_index].raw_idata)[i] = inportw((*channel).io_ports[ATA_REG_DATA]);
	}

	ata_process_indentify_data(device_index, channel);

	//kprintf("#%i INDETIFY OK\n", device_index);

	return EOK;
}

/**
 * Helper routine for convert words from IDENTIFY structure to
 * normal ASCII string.
 * @param buf		Pointer for output buffer (ASCII)
 * @param source	Array of words loaded from IDENTIFY command
 * @param count		Count of words that would be processed
 */
void ata_identify_to_ascii(char * buf, uint16_t * source, size_t count) {
	uint16_t * tmp = (uint16_t *)buf;
	unsigned i;
	for (i=0; i<count; i++) {
		*tmp = SWAPW(source[i]);
		tmp++;
	}
	buf[(count*2)] = '\0'; // don't forget terminate string :))
}

/**
 * Helper routine to interpret data from IDENTIFTY structure.
 * @param	device_index	0 - master, 1 - slave
 * @param	channel			Link to channel with devices
 */
void ata_process_indentify_data(int device_index, ata_channel_t * channel) {
	// process data and saves it to device_info
	(*channel).devices[device_index].device_info.is_ata = GETBITW((*channel).devices[device_index].raw_idata.gen_conf, 15);
	(*channel).devices[device_index].device_info.is_removable = GETBITW((*channel).devices[device_index].raw_idata.gen_conf, 7);

	(*channel).devices[device_index].device_info.logical_cylinders = (uint16_t) (*channel).devices[device_index].raw_idata.logical_cylinders;
	(*channel).devices[device_index].device_info.logical_head = (uint16_t) (*channel).devices[device_index].raw_idata.logical_head;
	(*channel).devices[device_index].device_info.logical_sectors_per_track = (uint16_t) (*channel).devices[device_index].raw_idata.logical_sectors_per_track;
	// model number conv
	ata_identify_to_ascii((*channel).devices[device_index].device_info.model_name, (*channel).devices[device_index].raw_idata.model_name, 20);
	// firmware conv
	ata_identify_to_ascii((*channel).devices[device_index].device_info.firmaware_rev, (*channel).devices[device_index].raw_idata.firmware_ver, 4);
	// serial number conv
	ata_identify_to_ascii((*channel).devices[device_index].device_info.serial_number, (*channel).devices[device_index].raw_idata.serial_number, 10);

	(*channel).devices[device_index].device_info.max_multi_sec_per_block = (uint8_t) GETBYTEW((*channel).devices[device_index].raw_idata.max_rw_multiple, 0); // MAX value for SET MULTIPLE MODE
	(*channel).devices[device_index].device_info.stanby_timer_support = (bool) GETBITW((*channel).devices[device_index].raw_idata.caps1, 13);
	(*channel).devices[device_index].device_info.IORDY_support = (bool) GETBITW((*channel).devices[device_index].raw_idata.caps1, 11);
	(*channel).devices[device_index].device_info.IORDY_disabled = (bool) GETBITW((*channel).devices[device_index].raw_idata.caps1, 10);

	// try detect max PIO mode supported...
	(*channel).devices[device_index].device_info.pio_mode = (uint8_t) GETBYTEW((*channel).devices[device_index].raw_idata.pio_data_mode_num, 1);
	// check if words 64 - 70 are valid
	if ( GETBITW((*channel).devices[device_index].raw_idata.validity, 1) == 1 )
		if ( (uint8_t) GETBITW((*channel).devices[device_index].raw_idata.adv_pio_modes, 0) == 1 )
			(*channel).devices[device_index].device_info.pio_mode = 3;
		else if ( (uint8_t) GETBITW((*channel).devices[device_index].raw_idata.adv_pio_modes, 1) == 1 )
			(*channel).devices[device_index].device_info.pio_mode = 4;

	(*channel).devices[device_index].device_info.cur_logical_cylinders = (uint16_t) (*channel).devices[device_index].raw_idata.cur_logical_cylinders;
	(*channel).devices[device_index].device_info.cur_logical_head = (uint16_t) (*channel).devices[device_index].raw_idata.cur_logical_head;
	(*channel).devices[device_index].device_info.cur_logical_sectors_per_track = (uint16_t) (*channel).devices[device_index].raw_idata.cur_logical_sectors_per_track;

	(*channel).devices[device_index].device_info.valid_multi_sec_per_intrr = GETBITW((*channel).devices[device_index].raw_idata.cur_rw_multiple, 8);
	(*channel).devices[device_index].device_info.cur_multi_sec_per_intrr = GETBYTEW((*channel).devices[device_index].raw_idata.cur_rw_multiple, 0);

	(*channel).devices[device_index].device_info.capacity_chs = MAKEDW((*channel).devices[device_index].raw_idata.cur_sec_cap_l, (*channel).devices[device_index].raw_idata.cur_sec_cap_h);
	(*channel).devices[device_index].device_info.capacity_lba28 = MAKEDW((*channel).devices[device_index].raw_idata.total_lba28_sec_l, (*channel).devices[device_index].raw_idata.total_lba28_sec_h);
	(*channel).devices[device_index].device_info.capacity_lba48 = MAKEQW((*channel).devices[device_index].raw_idata.total_lba48_sec_3, (*channel).devices[device_index].raw_idata.total_lba48_sec_2,
			(*channel).devices[device_index].raw_idata.total_lba48_sec_1, (*channel).devices[device_index].raw_idata.total_lba48_sec_0);

	// Try to detect supported addressing mode
	// device support only CHS
	(*channel).devices[device_index].device_info.addr_mode = ATA_CHS;
	(*channel).devices[device_index].device_info.max_addr_capacity = (uint64_t) (*channel).devices[device_index].device_info.capacity_chs;
	if (GETBITW((*channel).devices[device_index].raw_idata.caps1, 9) == 1) {
		// device support only LBA-28
		(*channel).devices[device_index].device_info.addr_mode = ATA_LBA_28;
		(*channel).devices[device_index].device_info.max_addr_capacity = (uint64_t) (*channel).devices[device_index].device_info.capacity_lba28;
	}
	if ((GETBITW((*channel).devices[device_index].raw_idata.com_set1, 10) == 1) && (GETBITW((*channel).devices[device_index].raw_idata.com_set_enb2, 10) == 1) ) {
		// device support LBA-48
		(*channel).devices[device_index].device_info.addr_mode = ATA_LBA_48;
		(*channel).devices[device_index].device_info.max_addr_capacity = (uint64_t) (*channel).devices[device_index].device_info.capacity_lba48 + 1;
	}

	uint8_t tmp = GETBYTEW((*channel).devices[device_index].raw_idata.mw_dma_mode, 0) & 0x07;
	unsigned i = 3;
	while ( i>0 && (GETBITW(tmp, i) == 0) )
		i--;
	(*channel).devices[device_index].device_info.multi_dma_support = i;

	tmp = GETBYTEW((*channel).devices[device_index].raw_idata.mw_dma_mode, 1) & 0x07;
	i = 3;
	while ( i>0 && (GETBITW(tmp, i) == 0) )
		i--;
	(*channel).devices[device_index].device_info.multi_dma_selected = i;

	i = 14;
	while ( i>0 && (GETBITW((*channel).devices[device_index].raw_idata.ata_version_major, i) == 0) )
		i--;
	(*channel).devices[device_index].device_info.highest_ata_support = i;

	i = 6;
	while ( i>0 && (GETBITW((*channel).devices[device_index].raw_idata.udma, i) == 0) )
		i--;
	(*channel).devices[device_index].device_info.highest_udma_support = i;

	tmp = GETBYTEW((*channel).devices[device_index].raw_idata.udma, 1);
	i = 6;
	while ( i>0 && (GETBITW(tmp, i) == 0) )
		i--;
	(*channel).devices[device_index].device_info.udma_selected = i;

	(*channel).devices[device_index].device_info.smart_enabled = (bool) GETBITW((*channel).devices[device_index].raw_idata.com_set_enb1, 0);
	(*channel).devices[device_index].device_info.smart_support = (bool) GETBITW((*channel).devices[device_index].raw_idata.com_set0, 0);
}

/**
 * Identify packet device with CMD INDETIFY_PACKET (ATAPI devices).
 * Data saves to channel->device->raw_ident.
 *
 * @return Standart errors codes.
 */
int ata_indetify_packet_device(int device_index, ata_channel_t * channel) {
	uint8_t sr_reg;
	unsigned i;

	//kprintf("DC port: %x\n", (*channel).io_ports[ATA_REG_DEVC]);
	outportb((*channel).io_ports[ATA_REG_DEVC], 0);

	if (ata_wait_for_status(0, ATA_REG_SR_BSY, channel, &sr_reg) != EOK)					// wait for bsy=0
		return ETIMEOUT;

	outportb((*channel).io_ports[ATA_REG_CMD], ATA_CMD_IDENTIFY_PACKET_DEVICE);				// send command
	ATA_DELAY_400NS;

	if (ata_wait_for_status(0, ATA_REG_SR_BSY, channel, &sr_reg) != EOK)					// wait for bsy=0
		return ETIMEOUT;

	if ((sr_reg & ATA_REG_SR_ERR) != 0)
		return EIO;			// some error occurs - exit

	if (ata_wait_for_status(ATA_REG_SR_DRQ, ATA_REG_SR_BSY, channel, &sr_reg) != EOK)		// wait for bsy=0, drq=1
		return ETIMEOUT;

	// read data from data register
	for (i = 0; i < ATA_INDETIFY_SIZE; i++) {
		((uint16_t *) &(*channel).devices[device_index].raw_idata)[i] = inportw((*channel).io_ports[ATA_REG_DATA]);
	}

	ata_process_indentify_data(device_index, channel);

	//kprintf("#%i INDETIFY PCK OK\n", device_index);

	return EOK;
}

/**
 * Reset all devices connected to channel.
 * @param channel				Channel for reset
 * @param use_interrupt			Vyvolavat preruseni po provedeni prikazu?
 */
PRIVATE int ata_soft_reset(ata_channel_t * channel, bool use_interrupt) {
	uint8_t dev_ctrl = use_interrupt ? 0 : ATA_REG_DEVC_NIEN;

	outportb((*channel).io_ports[ATA_REG_DEVC], dev_ctrl | ATA_REG_DEVC_SRST);
	ATA_DELAY_400NS;

	outportb((*channel).io_ports[ATA_REG_DEVC], dev_ctrl);
	ATA_DELAY_400NS;

	if (ata_wait_for_status(0, ATA_REG_SR_BSY, channel, NULL) != EOK)					// wait for bsy=0
		return ETIMEOUT;

	return EOK;
}

/**
 * Read sectors from physical ATA device.
 *
 * @param dev		Pointer to device structure.
 * @param lba		LBA address of first sector to read (LBA48 supported).
 * @param count		Number of sectors to read (up to 256 or 65536 (LBA48) sectors).
 * @param buffer	Memory buffer for holding readed data.
 *
 * @return			Standart error codes (EOK, EIO, ETIMEOUT).
 */
PUBLIC int ata_cmd_read(ata_device_t * dev, uint64_t lba, size_t count, uint16_t * buffer) {
	block_addr_t 	ba;
	uint8_t	 		sr_reg;
	uint8_t	 		asr_reg;
	uint16_t 		read_in_one_cycle;
	uint32_t		read_total = 0;

	// we can read max 256 or 65536 (LBA48) sectors in one read command
	if ( (count == 0) || (count > ((dev->device_info.addr_mode > ATA_LBA_48) ? ATA_SC_MAX_LBA48 : ATA_SC_MAX)) )
		return EINVAL;

	// calculate sector address registers for device
	ata_trans_lba(dev, lba, &ba);

	// select device, and fill DH regs 4bit of LBA address if needed.
	if (select_device(dev->channel_position, dev->device_channel,
			((dev->device_info.addr_mode > ATA_CHS) ? ATA_REG_DH_LBA_MODE : 0) | (ba.dh & 0xF) ) != EOK)
		return ETIMEOUT;

	// send address values to device address registers (for CHS, LBA28 if 256 sectors then send 0)
	ata_fill_addr_regs(dev, &ba, (dev->device_info.addr_mode < ATA_LBA_48 && count == ATA_SC_MAX ) ? 0 : count);

	// send READ MULTIPLE command to device
	outportb(dev->device_channel->io_ports[ATA_REG_CMD],
			(dev->device_info.addr_mode > ATA_LBA_48) ? ATA_READ_MULTIPLE_EXT : ATA_READ_MULTIPLE);
	ATA_DELAY_400NS;			// it's recommended for old devices

	// read cycle
	while (count > 0) {

		asr_reg = inportb(dev->device_channel->io_ports[ATA_REG_ASR]);		// read Alternate Status register and ignore results

		if (ata_wait_for_status(ATA_REG_SR_DRQ, ATA_REG_SR_BSY, dev->device_channel, &sr_reg) != EOK) {		// wait for bsy=0, drq=1
			kprintf("sr_reg: %x \n", sr_reg);
			kprintf("error_reg: %x \n", inportb(dev->device_channel->io_ports[ATA_REG_SR_ERR]));
			return ETIMEOUT;
		}

		if (sr_reg & ATA_REG_SR_ERR)		// read Error Status register
			return EIO;

		// calculate count of sectors that we will read in this cycle
		if (count > dev->device_info.max_multi_sec_per_block)
			read_in_one_cycle = dev->device_info.max_multi_sec_per_block;
		else
			read_in_one_cycle = count;

		inportsw(dev->device_channel->io_ports[ATA_REG_DATA], buffer, read_in_one_cycle * 256);		// reads sectors, one sector is 256 words

		buffer += read_in_one_cycle * 256;			// increase buffer pointer
		count = count - read_in_one_cycle;			// decrease count of sector that we already reads
		read_total += read_in_one_cycle;

	}

	kprintf("read total: %i \n", read_total);

	return EOK;
}

/**
 * Write sectors to physical ATA device.
 *
 * @param dev		Pointer to device structure.
 * @param lba		LBA address of first sector to read (LBA48 supported).
 * @param count		Number of sectors to write (up to 256 or 65536 (LBA48) sectors).
 * @param buffer	Memory buffer for holding data to write.
 *
 * @return			Standart error codes (EOK, EIO, ETIMEOUT).
 */
PUBLIC int ata_cmd_write(ata_device_t * dev, uint64_t lba, size_t count, uint16_t * buffer) {
	block_addr_t 	ba;
	uint8_t	 		sr_reg;
	uint8_t	 		asr_reg;
	uint16_t 		write_in_one_cycle;
	uint32_t		write_total = 0;

	// we can write max 256 or 65536 (LBA48) sectors in one read command
	if ( (count == 0) || (count > ((dev->device_info.addr_mode > ATA_LBA_48) ? ATA_SC_MAX_LBA48 : ATA_SC_MAX)) )
		return EINVAL;

	// calculate sector address registers for device
	ata_trans_lba(dev, lba, &ba);

	// select device, and fill DH regs 4bit of LBA address if needed.
	if (select_device(dev->channel_position, dev->device_channel,
			((dev->device_info.addr_mode > ATA_CHS) ? ATA_REG_DH_LBA_MODE : 0) | (ba.dh & 0xF) ) != EOK)
		return 10;

	// send address values to device address registers (for CHS, LBA28 if 256 sectors then send 0)
	ata_fill_addr_regs(dev, &ba, (dev->device_info.addr_mode < ATA_LBA_48 && count == ATA_SC_MAX ) ? 0 : count);

	// send WRITE MULTIPLE (EXT) command to device
	outportb(dev->device_channel->io_ports[ATA_REG_CMD],
			(dev->device_info.addr_mode > ATA_LBA_48) ? ATA_WRITE_MULTIPLE_EXT : ATA_WRITE_MULTIPLE);
	ATA_DELAY_400NS;			// it's recommended for old devices

	// read cycle
	while (count > 0) {

		asr_reg = inportb(dev->device_channel->io_ports[ATA_REG_ASR]);		// read Alternate Status register and ignore results

		if (ata_wait_for_status(ATA_REG_SR_DRQ, ATA_REG_SR_BSY, dev->device_channel, &sr_reg) != EOK) {		// wait for bsy=0, drq=1
			kprintf("sr_reg: %x \n", sr_reg);
			kprintf("error_reg: %x \n", inportb(dev->device_channel->io_ports[ATA_REG_SR_ERR]));
			return 11;
		}

		if (sr_reg & ATA_REG_SR_ERR)		// read Error Status register
			return 12;

		// calculate count of sectors that we will write in this cycle
		if (count > dev->device_info.max_multi_sec_per_block)
			write_in_one_cycle = dev->device_info.max_multi_sec_per_block;
		else
			write_in_one_cycle = count;

		outportsw(dev->device_channel->io_ports[ATA_REG_DATA], buffer, write_in_one_cycle * 256);		// write sectors, one sector is 256 words

		buffer += write_in_one_cycle * 256;			// increase buffer pointer
		count = count - write_in_one_cycle;			// decrease count of sector that we already reads
		write_total += write_in_one_cycle;

	}

	kprintf("write total: %i \n", write_total);

	return EOK;
}

/**
 * Transform lba address to address valid for selected device (CHS, LBA28, LBA48)
 * @param dev		Pointer to device structure.
 * @param lba		LBA address of first sector to read (LBA48 supported).
 * @param ba		Block address structure, separated for device input address registers.
 *
 * @return			EOK, EINVAL if address is out of device addressable space.
 */
PRIVATE int ata_trans_lba(ata_device_t * dev, uint64_t lba, block_addr_t * ba) {
	uint64_t c, i;

	/* check if address is in boundary of device */
	if (lba >= dev->device_info.max_addr_capacity)
		return EINVAL;

	switch (dev->device_info.addr_mode) {
		case ATA_CHS:
			c = (uint32_t) lba / (dev->device_info.logical_head * dev->device_info.logical_sectors_per_track );
			i = (uint32_t) lba % (dev->device_info.logical_head * dev->device_info.logical_sectors_per_track );
			ba->cl = c & 0xFF;
			ba->ch = (c >> 8) & 0xFF;
			ba->dh = ((uint32_t) i / dev->device_info.logical_sectors_per_track ) & 0xF;
			ba->sn = (1 + ((uint32_t) i % dev->device_info.logical_sectors_per_track )) & 0xFF;
			break;
		case ATA_LBA_28:
			ba->lba_low  = lba & 0xFF;				/*<< bits 7:0 */
			ba->lba_mid  = (lba >> 8)  & 0xFF;		/*<< bits 15:8 */
			ba->lba_high = (lba >> 16) & 0xFF;		/*<< bits 23:16 */
			ba->dh 		= (lba >> 24) & 0xF;		/*<< bits 28:24 */
			break;
		case ATA_LBA_48:
			ba->lba_low   = lba & 0xFF;				/*<< bits 7:0 */
			ba->lba_mid   = (lba >> 8)  & 0xFF;		/*<< bits 15:8 */
			ba->lba_high  = (lba >> 16) & 0xFF;		/*<< bits 23:16 */
			ba->lba_low1  = (lba >> 24) & 0xFF;		/*<< bits 31:24 */
			ba->lba_mid1  = (lba >> 32) & 0xFF;		/*<< bits 39:32 */
			ba->lba_high1 = (lba >> 40) & 0xFF;		/*<< bits 47:40 */
			ba->dh = 0;
	}

	return EOK;
}

/**
 * Write sector address to device address registers.
 * @param dev		Pointer to device structure.
 * @param ba		Block address structure, separated for device input address registers.
 * @param sc		Number of sectors to read or write.
 *
 * @note			Device / head register must be filled separately.
 */
PRIVATE void ata_fill_addr_regs(ata_device_t * dev, block_addr_t * ba, uint16_t sc) {
	// write high bits
	// Devices supporting LBA-48 have address registers as FIFO, so we can write two values to them.
	if (dev->device_info.addr_mode == ATA_LBA_48) {
		outportb(dev->device_channel->io_ports[ATA_REG_SC], sc >> 8);
		outportb(dev->device_channel->io_ports[ATA_REG_SN], ba->lba_low1);
		outportb(dev->device_channel->io_ports[ATA_REG_CL], ba->lba_mid1);
		outportb(dev->device_channel->io_ports[ATA_REG_CH], ba->lba_high1);
	}

	// write low bits
	outportb(dev->device_channel->io_ports[ATA_REG_SC], sc & 0xFF);
	outportb(dev->device_channel->io_ports[ATA_REG_SN], ba->lba_low);
	outportb(dev->device_channel->io_ports[ATA_REG_CL], ba->lba_mid);
	outportb(dev->device_channel->io_ports[ATA_REG_CH], ba->lba_high);
}

/**
 * Helper rutine shows debug information about ATA controllers and devices;
 */
void ata_show_info() {
	unsigned i, y, x;
	for (i=0; i < _ata.count; i++) {
		kprintf("ATA controller #%i (vid: 0x%x, did: 0x%x)\n", i, _ata.ata_ctrls[i].vendor_id, _ata.ata_ctrls[i].device_id);
		for (y=0; y < ATA_CHANNELS_PER_LEGACY_CTRL; y++)
			for (x=0; x < ATA_DEVICES_PER_CHANNEL; x++)
				if (_ata.ata_ctrls[i].channels[y].devices[x].device_type != ATA_DEV_NONE) {

					kprintf(" ata device %i => at channel: %i, type: %i, SMART: %i\\%i\n", x, y, _ata.ata_ctrls[i].channels[y].devices[x].device_type,
							_ata.ata_ctrls[i].channels[y].devices[x].device_info.smart_support,
							_ata.ata_ctrls[i].channels[y].devices[x].device_info.smart_enabled);
					kprintf("    - Cylinders: %i, Heads: %i, Sectors per track: %i\n", _ata.ata_ctrls[i].channels[y].devices[x].raw_idata.logical_cylinders,
							_ata.ata_ctrls[i].channels[y].devices[x].raw_idata.logical_head, _ata.ata_ctrls[i].channels[y].devices[x].raw_idata.logical_sectors_per_track);
					kprintf("    - Name: %s", _ata.ata_ctrls[i].channels[y].devices[x].device_info.model_name);
					kprintf("(%i MB)\n", (_ata.ata_ctrls[i].channels[y].devices[x].device_info.max_addr_capacity * 512) / 1024 / 1024);
					kprintf("    - Modes: ATA-%i,UltraDMA-%i\\%i,PIO-%i,MultiDMA-%i\\%i\n", _ata.ata_ctrls[i].channels[y].devices[x].device_info.highest_ata_support,
							_ata.ata_ctrls[i].channels[y].devices[x].device_info.highest_udma_support,
							_ata.ata_ctrls[i].channels[y].devices[x].device_info.udma_selected,
							_ata.ata_ctrls[i].channels[y].devices[x].device_info.pio_mode,
							_ata.ata_ctrls[i].channels[y].devices[x].device_info.multi_dma_support,
							_ata.ata_ctrls[i].channels[y].devices[x].device_info.multi_dma_selected);
					//kprintf("cap CHS: %i, cap lba28: %i, cap lba48: %l, max: %l\n", _ata.ata_ctrls[i].channels[y].devices[x].device_info.capacity_chs, _ata.ata_ctrls[i].channels[y].devices[x].device_info.capacity_lba28,
					//		_ata.ata_ctrls[i].channels[y].devices[x].device_info.capacity_lba48, _ata.ata_ctrls[i].channels[y].devices[x].device_info.max_addr_capacity);
					//kprintf("    - S\N: %s\n", _ata.ata_ctrls[i].channels[y].devices[x].device_info.serial_number);
					//kprintf("    - Firmware: %s\n", _ata.ata_ctrls[i].channels[y].devices[x].device_info.firmaware_rev);
					kprintf("    - Max r/w multiple sectors: %i, adrressing mode: %i\n", _ata.ata_ctrls[i].channels[y].devices[x].device_info.max_multi_sec_per_block,
							_ata.ata_ctrls[i].channels[y].devices[x].device_info.addr_mode);

				}
	}
}

/**
 * Helper rutine, test read/write to defined device
 */
void ata_rw_test(int dev) {
	int er = 0;

	if (_ata.ata_ctrls[0].channels[0].devices[dev].device_type == ATA_DEV_PATA) {
		kprintf("Test w/r to disk %i on channel 0\n", dev);
		uint16_t* sektor1 = (uint16_t*) kmalloc(512);
		memset(sektor1, 0, 512);
		strcpy(sektor1, "SEKTOR #1");

		uint16_t* sektor2 = (uint16_t*) kmalloc(512);
		memset(sektor2, 0, 512);
		strcpy(sektor2, "SEKTOR #2");

		// WRITE
		er = ata_cmd_write(&(_ata.ata_ctrls[0].channels[0].devices[dev]), (uint64_t) 1, 1, sektor1);
		kprintf("Result of test write #1: %i.\n", er);
		er = ata_cmd_write(&(_ata.ata_ctrls[0].channels[0].devices[dev]), (uint64_t) 2, 1, sektor2);
		kprintf("Result of test write #2: %i.\n", er);

		// READ
		memset(sektor1, 0, 512);
		memset(sektor2, 0, 512);
		er = ata_cmd_read(&(_ata.ata_ctrls[0].channels[0].devices[dev]), (uint64_t) 1, 1, sektor1);
		kprintf("Result of test read #1: %i.\n", er);
		er = ata_cmd_read(&(_ata.ata_ctrls[0].channels[0].devices[dev]), (uint64_t) 2, 1, sektor2);
		kprintf("Result of test read #2: %i.\n", er);

		kprintf("Result buffer 512B: %s\n", sektor1);
		kprintf("Result buffer 512B: %s\n", sektor2);

		kfree(sektor1);
		kfree(sektor2);
	}
}
